/*
 * ThreadDriveSequnce.cpp
 *
 *  Created on: Jan 31, 2013
 *      Author: Drew Prevost
 */

#include "ThreadDriveSequnce.h"

ThreadDriveSequnce::ThreadDriveSequnce(
                                        int nTachIrqNum,
                                        int nTachIsrPin,
                                        int nTachIsrMode,
                                        int nServoClutchPin,
                                        unsigned long nMaxServoClutchSpeedInMicrosPerDegree,
                                        unsigned int nInitialClutchPosition,
                                        unsigned int nInitialClutchRateInDegPerSec,
                                        int nDegreeClutchUp,
                                        int nRateClutchUp,
                                        int nDegreeServoClutchDown,
                                        int nRateServoClutchDown,
                                        int nServoThrottlePin,
                                        unsigned long nMaxServoThrottleSpeedInMicrosPerDegree,
                                        unsigned int nInitialThrottlePosition,
                                        unsigned int nInitialThrottleRateInDegPerSec,
                                        int nDegreeThrottleHigh,
                                        int nRateThrottleHigh,
                                        int nDegreeThrottleDown,
                                        int nRateThrottleDown,
                                        int nSecondsFullThrottle,
                                        int nSecondsIdle,
                                        int nLoadWheelMinRpm )
    : tachLoadWheel( Tachometer::singleton() ),
      servoClutch( nServoClutchPin, nMaxServoClutchSpeedInMicrosPerDegree, nInitialClutchPosition, nInitialClutchRateInDegPerSec ),
      servoThrottle( nServoThrottlePin, nMaxServoThrottleSpeedInMicrosPerDegree, nInitialThrottlePosition, nInitialThrottleRateInDegPerSec )
{
  tachLoadWheel.init( nTachIrqNum, nTachIsrPin, nTachIsrMode );

  this->nDegreeServoClutchDown = nDegreeServoClutchDown;
  this->nRateServoClutchDown = nRateServoClutchDown;
  this->nDegreeThrottleDown = nDegreeThrottleDown;
  this->nRateThrottleDown = nRateThrottleDown;
  this->nSecondsIdle = nSecondsIdle;
  this->nDegreeThrottleHigh = nDegreeThrottleHigh;
  this->nRateThrottleHigh = nRateThrottleHigh;
  this->nSecondsFullThrottle = nSecondsFullThrottle;
  this->nDegreeClutchUp = nDegreeClutchUp;
  this->nRateClutchUp = nRateClutchUp;
  this->nLoadWheelMinRpm = nLoadWheelMinRpm;
}

ThreadDriveSequnce::~ThreadDriveSequnce()
{
}

bool ThreadDriveSequnce::run()
{
  PT_BEGIN()
  ;

  // Step one:  set throttle to idle, and clutch down, and wait until those actions have been completed
  servoClutch.setPosition( nDegreeServoClutchDown, nRateServoClutchDown );
  servoThrottle.setPosition( nDegreeThrottleDown, nRateThrottleDown );
  PT_WAIT_UNTIL( (servoClutch.getPosition() == nDegreeServoClutchDown) && (servoThrottle.getPosition() == nDegreeThrottleDown) );

  // Step two:  wait a bit
  threadMicroDelay.setDelay( nSecondsIdle * 100000 );
  PT_SPAWN( threadMicroDelay );

  // Step three:  set throttle to high and wait until complete
  servoThrottle.setPosition( nDegreeThrottleHigh, nRateThrottleHigh );
  PT_WAIT_UNTIL( servoThrottle.getPosition() == nDegreeThrottleHigh );

  // Step four:  wait for a bit at this sate
  threadMicroDelay.setDelay( nSecondsFullThrottle * 100000 );
  PT_SPAWN( threadMicroDelay );

  // Step five:  set clutch to low, and wait to complete, then set throttle to low an wait until complete
  servoClutch.setPosition( nDegreeClutchUp, nRateClutchUp );
  PT_WAIT_UNTIL( servoClutch.getPosition() == nDegreeThrottleDown );
  servoThrottle.setPosition( nDegreeThrottleDown, nRateClutchUp );
  PT_WAIT_UNTIL( servoThrottle.getPosition() == nDegreeThrottleDown );

  // Step five:  wait for load wheel to slow down
  PT_WAIT_UNTIL( tachLoadWheel.getRpm() <= nLoadWheelMinRpm );

PT_END()
;
return false;
}

